#include "MyFFT.h"
#include <math.h>
#include <string.h>

 
static const float PI = 3.1415926535897932384626f;
 
#define sq(x) ((x)*(x))
static void DCRemoval(struct MFFT *self);
 
/*******************************************************************************
 * 函 数 名         : Init
 * 函数功能         : fft对象各参数初始化
 * 输入参数         : *vReal --> 信号输入、输出向量指针，保存fft计算的结果（实部）
 *                   *vImag --> 信号输入、输出向量指针，保存fft计算的结果（虚部）
 *                   samples--> 信号采样的个数
 *                   samplingFrequency    -->采样频率   hz
 * 返 回 值         : 无
 *******************************************************************************/
static void Init(struct MFFT *self, float *vReal, float *vImag, uint16_t samples, float samplingFrequency)
{
    self->_vReal = vReal;
    self->_vImag = vImag;
    self->_samples = samples;
    self->_samplingFrequency = samplingFrequency;
    self->_power = self->Exponent(self, samples);
    self->_step  = self->_samplingFrequency/self->_samples;
    memset(self->_vImag, 0, self->_samples * sizeof(float));
    DCRemoval(self);
}
/*******************************************************************************
 * 函 数 名         : Compute
 * 函数功能         : fft结果计算
 * 输入参数         : *vReal --> 信号输入、输出向量指针，保存fft计算的结果（实部）
 *                   *vImag --> 信号输入、输出向量指针，保存fft计算的结果（虚部）
 *                   samples--> 信号采样的个数
 *                    dir    -->
 * 返 回 值         : 1--> 失败   0 --> 成功
 *******************************************************************************/
static uint8_t Compute(struct MFFT *self)
{
 
    uint16_t j = 0;
    uint16_t i = 0;
    float temp;
    uint16_t k;
 
    float SR = -1.0;
    float SI = 0.0;
    float TR = 0;
    float TI = 0;
    float UR = 0;
    float UI = 0;
    uint16_t l2 = 1;
    uint16_t l1 = 0;
    uint16_t l = 0;
    uint16_t i1 = 0;
    uint8_t power = 0;
    uint16_t samples_1;
 
    if (self->_samples != 0x0001 << self->_power) //samples  不满足2的power次方 失败返回
    {
        return 1;
    }
 
    samples_1 = self->_samples - 1;
    /*时域分解  位反转*/
    for (i = 0; i < samples_1; i++)
    {
        if (i < j)
        {
            temp = self->_vReal[i];
            self->_vReal[i] = self->_vReal[j];
            self->_vReal[j] = temp;
        }
        k = (self->_samples >> 1);
        while (k <= j)
        {
            j -= k;
            k >>= 1;
        }
        j += k;
    }
 
    /*频域合成  */
 
    for (l = 0; l < self->_power; l++) //l=0 1 2 3
    {
        l1 = l2;  // 1 2 4  8
        l2 <<= 1; // 2 4 8 16
        UR = 1.0;
        UI = 0.0;
        SR = cos(PI / l1);
        SI = -sin(PI / l1);
        for (j = 0; j < l1; j++) //分组dft循环
        {
            for (i = j; i < self->_samples; i += l2) // 蝶形运算
            {
                i1 = i + l1;
                TR = UR * self->_vReal[i1] - UI * self->_vImag[i1];
                TI = UI * self->_vReal[i1] + UR * self->_vImag[i1];
                self->_vReal[i1] = self->_vReal[i] - TR;
                self->_vImag[i1] = self->_vImag[i] - TI;
                self->_vReal[i] += TR;
                self->_vImag[i] += TI;
            }
            TR = UR;
            UR = TR * SR - UI * SI;
            UI = TR * SI + UI * SR;
        }
    }
 
    temp = self->_samples / 2.0f;
    for (i = 0; i < self->_samples; i++) //幅值计算
    {
        self->_vReal[i] = self->_vReal[i] / temp;
        self->_vImag[i] = self->_vImag[i] / temp;
        self->_vReal[i] = sqrt(sq(self->_vReal[i])  + sq(self->_vImag[i]))*self->_revalue;  //保存幅值的值
        self->_vImag[i] =  self->_step *i;                                                  //保存对应频率的值
    }
    return 0;
}
/*******************************************************************************
 * 函 数 名         : Exponent
 * 函数功能         : 求 value 是2的多少次方
 * 输入参数         : value --> 待求的值
 * 返 回 值         : 返回 幂 值
 *******************************************************************************/
static uint8_t Exponent(struct MFFT *self, uint16_t value)
{
    uint8_t result = 0;
    while (((value >> result) & 1) != 1)
        result++;
    return (result);
}
/*******************************************************************************
 * 函 数 名         : Windowing
 * 函数功能         : 给输入的信号加窗
 * 输入参数         : windowType --> 窗口类型
 * 返 回 值         : 返回 幂 值
 *******************************************************************************/
static void Windowing(struct MFFT *self,  enum WinType windowType)
{
  uint16_t i ;
  float samplesMinusOne = (float)(self->_samples) - 1.0;
	for ( i = 0; i < (self->_samples >> 1); i++)
	{
		float indexMinusOne = (float)i;
		float ratio = (indexMinusOne / samplesMinusOne);
		float weighingFactor = 1.0;
		// Compute and record weighting factor
		switch (windowType)
		{
		case FFT_WIN_TYP_RECTANGLE: // rectangle (box car)
			weighingFactor = 1.0;
            self->_revalue = 1;
			break;
		case FFT_WIN_TYP_HAMMING: // hamming
			weighingFactor = 0.54 - (0.46 * cos(twoPi * ratio));
            self->_revalue = 1.852;
			break;
		case FFT_WIN_TYP_HANN: // hann
			weighingFactor = 0.54 * (1.0 - cos(twoPi * ratio));
            self->_revalue = 2;
			break;
		case FFT_WIN_TYP_TRIANGLE: // triangle (Bartlett)
			weighingFactor = 1.0 - ((2.0 * abs(indexMinusOne - (samplesMinusOne / 2.0))) / samplesMinusOne);
            self->_revalue = 2;
			break;
		case FFT_WIN_TYP_NUTTALL: // nuttall
			weighingFactor = 0.355768 - (0.487396 * (cos(twoPi * ratio))) + (0.144232 * (cos(fourPi * ratio))) - (0.012604 * (cos(sixPi * ratio)));
            self->_revalue = 1;
			break;
		case FFT_WIN_TYP_BLACKMAN: // blackman
			weighingFactor = 0.42323 - (0.49755 * (cos(twoPi * ratio))) + (0.07922 * (cos(fourPi * ratio)));
            self->_revalue = 2.381;
			break;
		case FFT_WIN_TYP_BLACKMAN_NUTTALL: // blackman nuttall
			weighingFactor = 0.3635819 - (0.4891775 * (cos(twoPi * ratio))) + (0.1365995 * (cos(fourPi * ratio))) - (0.0106411 * (cos(sixPi * ratio)));
            self->_revalue = 1;
			break;
		case FFT_WIN_TYP_BLACKMAN_HARRIS: // blackman harris
			weighingFactor = 0.35875 - (0.48829 * (cos(twoPi * ratio))) + (0.14128 * (cos(fourPi * ratio))) - (0.01168 * (cos(sixPi * ratio)));
            self->_revalue = 1;
			break;
		case FFT_WIN_TYP_FLT_TOP: // flat top
			weighingFactor = 0.2810639 - (0.5208972 * cos(twoPi * ratio)) + (0.1980399 * cos(fourPi * ratio));
            self->_revalue = 1;
			break;
		case FFT_WIN_TYP_WELCH: // welch
			weighingFactor = 1.0 - sq((indexMinusOne - samplesMinusOne / 2.0) / (samplesMinusOne / 2.0));
            self->_revalue = 1;
			break;
		}
 
        self->_vReal[i] *= weighingFactor;
        self->_vReal[self->_samples - (i + 1)] *= weighingFactor;
	}
}
/*******************************************************************************
 * 函 数 名         : DCRemoval
 * 函数功能         : 消除直流分量
 * 输入参数         :  
 * 返 回 值         : 
 *******************************************************************************/
static void DCRemoval(struct MFFT *self)
{
	/*计算信号的平均值*/
	float mean = 0;
	uint16_t i =0;
	for ( i = 0; i < self->_samples; i++)
	{
		mean += self->_vReal[i];
	}
	mean /= self->_samples;
	/*信号依次减去平均值*/
	for (i = 0; i < self->_samples; i++)
	{
		self->_vReal[i] -= mean;
	}
}
/*******************************************************************************
 * 函 数 名         : MFFT_Create
 * 函数功能         : 创建fft对象
 * 输入参数         :  
 * 返 回 值         : 返回 幂 值
 *******************************************************************************/
void MFFT_Create(struct MFFT *self)
{
    memset(self, 0, sizeof(struct MFFT));
 
    self->Init = Init;
    self->Compute = Compute;
    self->Exponent = Exponent;
    self-> Windowing= Windowing;
}